#include "../handler_task.h"
#include "../../driver/powder/box_motors.h"
#include "../../driver/powder/cargo_motor.h"
#include "../../driver/powder/move_motor.h"
#include "../../driver/powder/sensor.h"
#include "../../service/salve_conn.h"
#include "../../conn/proto.h"

namespace task {

using namespace conn;
using namespace service;
using namespace driver;
using namespace utils;

static Error on_updown_ctrl(const Message &msg) {
    uint8_t type;
    Error err = msg.parse(type);
    if (err) {
        goto finish;
    }
    switch (type) {
        case 0: err = boxMotors->up(); break;
        case 1: err = boxMotors->down(); break;
        case 2: err = boxMotors->up(200); break;
        case 3: err = boxMotors->down(200); break;
        default: err = ec::INVALID; break;
    }
finish:
    return err;
}

static Error on_dump_ctrl(const Message &msg) {
    uint8_t type;
    Error err = msg.parse(type);
    if (err) {
        goto finish;
    }
    switch (type) {
        case 0: err = boxMotors->dump(); break;
        case 1: err = boxMotors->dump_reset(); break;
        case 2: err = boxMotors->dump(200); break;
        case 3: err = boxMotors->dump_reset(200); break;
        default: err = ec::INVALID; break;
    }
finish:
    return err;
}

static Error on_move_ctrl(const Message &msg) {
    uint16_t position;
    Error err = msg.parse(position);
    if (err) {
        goto finish;
    }
    err = moveMotor->run_with_mm(position);
finish:
    return err;
}

static Error on_cargo_ctrl(const Message &msg) {
    uint16_t position;
    Error err = msg.parse(position);
    if (err) {
        goto finish;
    }
    err = cargoMotor->run(position);
finish:
    return err;
}


static Error on_ped_ctrl(const Message &msg) {
    uint8_t type;
    Error err = msg.parse(type);
    if (err) {
        goto finish;
    }
    switch (type) {
        case 0: boxMotors->pde_lock(); break;
        case 1: boxMotors->pde_unlock(); break;
        default: err = ec::INVALID; break;
    }
finish:
    return err;
}

void HandlerTask::dispatch_impl(const Message &msg) {
    Error err;

    switch (msg.conn_info.req) {    
        case proto::REQ_POWDER_UPDOWM_CTRL:
            err = on_updown_ctrl(msg);
        break;

        case proto::REQ_POWDER_DUMP_CTRL:
            err = on_dump_ctrl(msg);
        break;

        case proto::REQ_POWDER_MOVE_CTRL:
            err = on_move_ctrl(msg);
        break;

        case proto::REQ_POWDER_CARGO_CTRL:
            err = on_cargo_ctrl(msg);
        break;

        case proto::REQ_POWDER_ELOCK_CTRL:
            err = on_ped_ctrl(msg);
        break;
    }

    salveConn->response(msg, err);
}

}
